package org.extremengine.collision;

import org.extremengine.World;
import org.extremengine.common.RigidBody;
import org.extremengine.common.Terrain;
import org.extremengine.common.Utils;
import org.extremengine.shape.Circle;
import org.extremengine.shape.HorizontalLine;
import org.extremengine.shape.ParallelRectangle;
import org.extremengine.shape.Shape;
import org.extremengine.shape.ShapeUtils;
import org.extremengine.shape.UnlimitedLine;
import org.extremengine.shape.VerticalLine;

/**
 * 测试两个刚体之间，刚体与地形之间是否碰撞
 * @author yuxing
 *
 */
public class CollisionDetector {
	
	// Reference to World which set in World(should be refactored later)
	public World world;
	
	// Some static bodies or sticky bodies never do collision test for each other, use removeRelation() to avoid test.
	public RelationGraph<RigidBody> relationGraph = new RelationGraph<RigidBody>();
	
	public AfterCollision afterCollision;
	
	public CollisionDetector(World world) {
		super();
		this.world = world;
		afterCollision = new AfterCollision(world.getSpeedValve(), world.getGravityAccelaration());
	}

	/**
	 * 测试刚体间碰撞
	 * @param rb1
	 * @param rb2
	 * @return
	 */
	public boolean detectCollision(RigidBody rb1, RigidBody rb2, long frameTimePeriod) {
		if(rb1 == rb2) {
			return false;
		}
		boolean isCollided = false;

		// Detect 2 Circles, if collide, retreat and calculate new velocity for them.
		if(rb1.getShape().isCircle() && rb2.getShape().isCircle()) {
			Circle c1 = (Circle) rb1.getShape();
			Circle c2 = (Circle) rb2.getShape();
			double heartDistance = c1.getHeart().distanceTo(c2.getHeart());
			double radiusplus = c1.getRadius() + c2.getRadius();

			if(heartDistance <= radiusplus) {
			
				isCollided =  true;
				
				// Retreat before collide calculation.
				afterCollision.retreatAsCircles(rb1, rb2);
				
				// Do collide calculation.
				afterCollision.onNonParallel(rb1, rb2);
			}
			return isCollided;
		}
		// Detect 1 Circle collide with other Shape('s AABB).
		else if (rb1.getShape().isCircle() || rb2.getShape().isCircle()) {
			
			RigidBody circleBody = rb1.getShape().isCircle() ? rb1: rb2;
			RigidBody rectBody = rb1.getShape().isCircle() ? rb2: rb1;
			
			Circle c = (Circle) circleBody.getShape();
			ParallelRectangle aabb = rectBody.getAabb();
			
			double x0 = c.getHeart().getX();
			double y0 = c.getHeart().getY();
			double r = c.getRadius();
			double deltax1 = 0;
			double deltay1 = 0;
			double deltax2 = 0;
			double deltay2 = 0;
			int collideState = afterCollision.COLLISION_STATE_NOCOLLISION;

			// Check circle cut line of AABB in clockwise.
			if (x0 >= aabb.getX1() && x0 <= aabb.getX2()) {
				double sy1 = circleBody.getV().getSpeedy();
				double sy2 = rectBody.getV().getSpeedy();
				double k1 = sy1 / Math.abs(sy1 - sy2);
				double k2 = sy2 / Math.abs(sy1 - sy2);
				double top = y0 + r;
				double bottom = y0 - r;
				double deltay = 0;
				// Circle is on below of the AABB. 
				if (bottom <= aabb.getY1()) {
					isCollided = top >= aabb.getY1();
					if (isCollided) {
						deltay = top - aabb.getY1();
					}
				}
				// Circle is on above of the AABB.
				else if (top >= aabb.getY2()) {
					isCollided = bottom <= aabb.getY2();
					if (isCollided) {
						deltay = aabb.getY2() - bottom;
					}
				}
				deltay1 = deltay * k1;
				deltay2 = deltay * k2;
				collideState = afterCollision.COLLISION_STATE_AXIS_X;
			}
			else if (y0 >= aabb.getY1() && y0 <= aabb.getY2()) {
				double sx1 = circleBody.getV().getSpeedx();
				double sx2 = rectBody.getV().getSpeedx();
				double k1 = sx1 / Math.abs(sx1 - sx2);
				double k2 = sx2 / Math.abs(sx1 - sx2);
				double deltax = 0;
				double left = x0 - r;
				double right = x0 + r;
				// Circle is at the left of the AABB.
				if (left <= aabb.getX1()) {
					isCollided = right >= aabb.getX1();
					if (isCollided) {
						deltax = right - aabb.getX1();
					}
				}
				// Circle is at the right of the AABB.
				else if (right >= aabb.getX2()) {
					isCollided = left <= aabb.getX2();
					if (isCollided) {
						deltax = aabb.getX2() - left;
					}
				}
				deltax1 = deltax * k1;
				deltax2 = deltax * k2;
				collideState = afterCollision.COLLISION_STATE_AXIS_Y;
			}
			// Check circle touch corner of AABB in clockwise.
			else{ 
				double x =0, y =0;
				if (x0 < aabb.getX1() && y0 > aabb.getY2()) {
					x = aabb.getX1(); y = aabb.getY2();
					isCollided = c.containsPoint(aabb.getX1(), aabb.getY2());
				}
				else if (x0 > aabb.getX2() && y0 > aabb.getY2()) {
					x = aabb.getX2(); y = aabb.getY2();
					isCollided = c.containsPoint(aabb.getX2(), aabb.getY2());
				}
				else if (x0 > aabb.getX2() && y0 < aabb.getY1()) {
					x = aabb.getX2(); y = aabb.getY1();
					isCollided = c.containsPoint(aabb.getX2(), aabb.getY1());
				}
				else if (x0 < aabb.getX1() && y0 < aabb.getY1()) {
					x = aabb.getX1(); y = aabb.getY1();
					isCollided = c.containsPoint(aabb.getX1(), aabb.getY1());
				}
				if (isCollided) {
					collideState = afterCollision.COLLISION_STATE_COMMON;
				}
			}
			if(isCollided) {
				// Calculate everything after collision.
				if(collideState == afterCollision.COLLISION_STATE_AXIS_X || collideState == afterCollision.COLLISION_STATE_AXIS_Y) {
					// Retreat from the offset.
					c.transform(-deltax1, -deltay1);
					aabb.transform(-deltax2, -deltay2);
					
					// Collides with parallel is easy to calculate.
					afterCollision.circleWithParallelLine(circleBody, rectBody, collideState, frameTimePeriod);
				}
				else {
					afterCollision.retreatAsCircles(rb1, rb2);
					// calculating after collision.
					afterCollision.onNonParallel(rb1, rb2);
				}
				return true;
			}
			else {
				return false;
			}
		}
		// Detect 2 non-circle rigid body collision.
		else if (rb1.getShape().isCircle() == false && rb1.getShape().isCircle() == false) {
			ParallelRectangle aabb1 = rb1.getAabb();
			ParallelRectangle aabb2 = rb2.getAabb();

			// 检测两个AABB在x轴和y轴上的投影是否相交
//			if (ShapeUtils.coordinateRangeOverlap(aabb1.getX1(), aabb1.getX2(), aabb2.getX1(), aabb2.getX2())
//					&& ShapeUtils.coordinateRangeOverlap(aabb1.getY1(), aabb1.getY2(), aabb2.getY1(), aabb2.getY2())) {
			double overlapx = aabb1.projectionOverlap(aabb2, Shape.COORDINATE_AXIS_X);
			double overlapy = aabb1.projectionOverlap(aabb2, Shape.COORDINATE_AXIS_Y);
			if(overlapx >=0 && overlapy >=0){
				// TODO
				// Retreat from overstep.
				afterCollision.retreatAsParallelRectangles(rb1, rb2, overlapx, overlapy);				
				
				// calculating after collision
				afterCollision.onParallel(rb1, rb2);
				
				
				return true;
			}

		}
		// Test 2 non-circle shape collision
		else {
			// TODO
			
		}

		return false;
	}
	
	/**
	 * 测试刚体与地形碰撞。
	 * Detect collision with terrain, if collide, transform position by terrain's position.
	 * @param rb
	 * @param terrain
	 * @return Collide or not
	 */
	public boolean detectCollision(RigidBody rb, Terrain terrain, long frameTimePeriod) {
		// Horizontal Line is easy to test.
		if (rb.isMovable() == false) {
			return false;
		}
		if (terrain.getShape() instanceof UnlimitedLine) {
			UnlimitedLine unlimitedLine = (UnlimitedLine) terrain.getShape();
			// With Circle
			if (rb.getShape() instanceof Circle) {
				Circle c = (Circle) rb.getShape();
				if (detectCirleWithUnlimitLine(unlimitedLine, c) == true) {
					if (afterCollision.withTerrain(rb, terrain, frameTimePeriod)) {
						// Sticky after collide.
						rb.stickWith(terrain);
					}
					else {
						rb.unstickWith(terrain);
					}
					// Determine collision if not sticky and 
					return true;
				}
			}
			// With Parallel Rectangle
			else if (rb.getShape() instanceof ParallelRectangle) {
				ParallelRectangle pr = (ParallelRectangle) rb.getShape();
				if (detectRectangleWithUnlimitLine(unlimitedLine, pr) == true) {
					if (afterCollision.withTerrain(rb, terrain, frameTimePeriod)) {
						// Sticky after collide
						rb.stickWith(terrain);
					}
					else {
						rb.unstickWith(terrain);
					}
					return true;
				}
			}
			else {
				return false;
			}
		}
		else {
			return false;
		}
		return false;
	}
	
	/**
	 * Circle collide with unlimited horizontal or vertical line. 
	 * @param line
	 * @param c
	 * @return
	 */
	protected boolean detectCirleWithUnlimitLine(UnlimitedLine line, Circle c) {
		double linePosition= 0.0; // Position of line in one axis
		double circlePosition = 0.0; // Position of circle in one axis
		if (line instanceof HorizontalLine) {
			linePosition = ((HorizontalLine) line).getY();
			circlePosition = c.getHeart().y;
		}
		else if(line instanceof VerticalLine) {
			linePosition = ((VerticalLine) line).getX();
			circlePosition = c.getHeart().x;
		}
		if (line.getFreeSpace() == 1 && (circlePosition - c.getRadius()) < linePosition) {
			// Let them back to back if collide.
			if (line instanceof HorizontalLine) {
				c.transform(0, linePosition - (circlePosition - c.getRadius()));
			}
			else {
				c.transform(linePosition - (circlePosition - c.getRadius()), 0);
			}
			return true;
		}
		else if (line.getFreeSpace() == -1 && (circlePosition + c.getRadius() > linePosition)) {
			// Let them back to back if collide.
			if (line instanceof HorizontalLine) {
				c.transform(0, linePosition - (circlePosition + c.getRadius()));
			}
			else {
				c.transform(linePosition - (circlePosition + c.getRadius()), 0);
			}
			return true;
		}
		return false;
	}

	/**
	 * Rectangle collide with unlimited horizontal or vertical line. 
	 * @param line
	 * @param c
	 * @return
	 */
	protected boolean detectRectangleWithUnlimitLine(UnlimitedLine line, ParallelRectangle pr) {
		double linePosition; // Position of line in one axis
		double rectPosition;
		if (line instanceof HorizontalLine) {
			linePosition = ((HorizontalLine) line).getY();
			rectPosition = line.getFreeSpace() == 1 ? pr.getY1() : pr.getY2();
		}
		else {
			linePosition = ((VerticalLine) line).getX();
			rectPosition = line.getFreeSpace() == 1 ? pr.getX1() : pr.getX2();
		}
		if ((line.getFreeSpace() == 1 && rectPosition < linePosition)
				|| (line.getFreeSpace() == -1 && rectPosition > linePosition)) {
			if (line instanceof HorizontalLine) {
				pr.transform(0, linePosition - rectPosition);
			}
			else {
				pr.transform(linePosition - rectPosition, 0);
			}
			return true;
		}
		return false;
	}
}
